function [msg_cycle_finished, alg_finished] = localization_msg_cycle_step(alg_param)
  msg_cycle_finished = true;  alg_finished = false;    global g1400_finished;
  global g1400_cur_node;
  global g1400_LMap;
  global g1400_LMap_next;
    if g1400_finished == 1  msg_cycle_finished = true;  alg_finished = true;  return;  end
    cur_node = g1400_cur_node;  % Macierz lokalizacji wezlow i ich statusow
  LMap = g1400_LMap;  % Typ wezla - latarnia
  latern = 2;    % Sprawdzamy czy pozycje wezla nalezy zaktualizowac (status = 1 i nie
  % latarnia)
  if LMap(cur_node, 1) == 1 && LMap(cur_node, 2) ~= latern  update_node(cur_node);  localization_draw_point(cur_node, g1400_LMap_next(cur_node, 2));  end
  end
  function update_node(cur_node)
  %% Aktualizacja pozycji wezla
    % Szukamy punktow odniesienia
  [anchors, success] = find_anchors(cur_node);  if ~success  return;  end
  % Estymujemy pozycje
  if_notify = estimate_position(cur_node, anchors);  if ~if_notify  return;  end
  % Dajemy znac sasiadom ze zaktualizowalismy pozycje
  notify(cur_node); end
  function [anchors, success] = find_anchors(cur_node)
  %% Szukanie kotwic
  global N_Map;
  global g1400_DDMap;
  global g1400_LMap;
  global g1400_epsilon;
  global g1400_gamma;
    % Macierz odleglosci
  DDMap = g1400_DDMap;  % Macierz lokalizacji
  LMap = g1400_LMap;    anchors = zeros(1,3);  success = false;    % Liczba sasiadow
  L = N_Map(cur_node, 1);  if L < 3  return;  end
    min_e = 1000000;  anchor = 0;  % Szukamy pierwszego wezla o najmniejszym e
  for i = 2:L+1  i_node = N_Map(cur_node, i, 1);  if find(anchors==i_node)  continue;
  end
  % Nie bierzemy pod uwage wezlow o typie 0 lub o tym samym polozeniu
  % lub stacji bazowej
  if LMap(i_node, 2) == 0 || DDMap(cur_node, i_node) == 0  continue;
  end
    e = LMap(i_node, 5);  if e < min_e  anchor = i_node;  min_e = e;  end
  end
    if anchor == 0  return;  else  anchors(1) = anchor;  end
    min_e = 1000000;  anchor = 0;  % Szukamy drugiego wezla o najmniejszym e
  for i = 2:L+1  i_node = N_Map(cur_node, i, 1);  if find(anchors==i_node)  continue;
  end
  % Nie bierzemy pod uwage wezlow o type 0 lub o tym samym polozeniu
  if LMap(i_node, 2) == 0 || DDMap(cur_node, i_node) == 0  continue;
  end
    % kryterium epsilon
  if dist([LMap(anchors(1), 3) LMap(anchors(1), 4)], [LMap(i_node, 3) LMap(i_node, 4)]) < g1400_epsilon  continue;
  end
    e = LMap(i_node, 5);  if e < min_e  anchor = i_node;  min_e = e;  end
  end
    if anchor == 0  return;  else  anchors(2) = anchor;  end
    min_e = 1000000;  anchor = 0;  % Szukamy trzeciego wezla o najmniejszym e
  for i = 2:L+1  i_node = N_Map(cur_node, i, 1);  if find(anchors==i_node)  continue;
  end
  % Nie bierzemy pod uwage wezlow o type 0 lub o tym samym polozeniu
  if LMap(i_node, 2) == 0 || DDMap(cur_node, i_node) == 0  continue;
  end
    % kryterium epsilon wzgledem poprzednich kotwic
  %if (dist([L_Map(anchors(1),3) L_Map(anchors(1), 4)], [L_Map(i,3) L_Map(i,4)]) > g1400_epsilon) || (dist([L_Map(anchors(2),3) L_Map(anchors(2), 4)], [L_Map(i,3) L_Map(i,4)]) > g1400_epsilon)
  % continue;
  %end
    % kryterium gamma
  if minAng([LMap(anchors(1),3) LMap(anchors(1),4)], [LMap(anchors(2),3) LMap(anchors(2),4)], [LMap(i_node,3) LMap(i_node,4)]) < g1400_gamma  continue;
  end
    e = LMap(i_node, 5);  if e < min_e  anchor = i_node;  min_e = e;  end
  end
    if anchor == 0  return;  else  anchors(3) = anchor;  end
    success = true; end
  function [notify] = estimate_position(cur_node, anchors)
  %% Obliczanie pozycji na podstawie kotwic
  global g1400_LMap_next;
  global g1400_LMap;
  global g1400_DDMap;
  global S_Map;
    DDMap = g1400_DDMap;  notify = false;  orig_pos = [S_Map(cur_node,1) S_Map(cur_node,2)];    r = zeros(1,3);    % Odleglosci kotwic od punktu
  for i=1:3  r(i) = DDMap(cur_node, anchors(i));  end
    % Pobieramy punty przeciecia okregow
  W = get_point(anchors(1), anchors(2), r(1), r(2), cur_node);  T = get_point(anchors(1), anchors(3), r(1), r(3), cur_node);  F = get_point(anchors(2), anchors(3), r(2), r(3), cur_node);   % if W(1) == 0 || T(1) == 0 || F(1) == 0
 % draw_wtf(cur_node, W,T,F, anchors, r);
 % end
    % Obliczamy pozycje punktu na podstawie W, T, F
  x = (W(1) + T(1) + F(1)) / 3;  y = (W(2) + T(2) + F(2)) / 3;  % Wyliczamy blad
  e = get_e([x y], W, T, F);    %if orig_pos(1) ~= x || orig_pos(2) ~= y
  % orig_pos
  % cur_pos = [x y]
  %end
    % Jezeli nie zmniejszyl sie blad zostawiamy w spokoju
  cur_e = g1400_LMap(cur_node, 5);  if cur_e <= e  return;  end
    % Aktualizujemy tablice dla nastepnej iteracji
  n_type = node_type(e);  g1400_LMap_next(cur_node, 2) = n_type;  g1400_LMap_next(cur_node, 3) = x;  g1400_LMap_next(cur_node, 4) = y;  g1400_LMap_next(cur_node, 5) = e;    % Jezeli obliczona pozycja zmienila sie powiadamiamy sasiadow
  if n_type > 0 && (g1400_LMap(cur_node, 3) ~= x || g1400_LMap(cur_node, 4) ~= y)  g1400_LMap_next(cur_node, 3) = x;  g1400_LMap_next(cur_node, 4) = y;  notify = true;  end
 end
  function [point] = get_point(p1, p2, r1, r2, p3)
  %% Szukanie punktu przeciecia okregow
  global g1400_LMap;
  global S_Map;
    LMap = g1400_LMap;    x1 = LMap(p1, 3);  y1 = LMap(p1, 4);  x2 = LMap(p2, 3);  y2 = LMap(p2, 4);    % Wspolrzedne trzeciego punktu
  x3 = S_Map(p3, 1);  y3 = S_Map(p3, 2);  % d12 = sqrt((x2-x1)^2 + (y2 - y1)^2);
 % 
 % beta = atan((y2 - y1)/(x2 - x1));
 % alfa = acos((r1^2 - r2^2 + d12^2)/(2*r1*d12));
 % 
 % x = x1 + r1*cos(alfa + beta);
 % y = y1 + r1*sin(alfa + beta);
 % 
 % point = [x, y]
   % Jezeli okregi nie przecinaja sie zwiekszamy ich promienie zeby mialy
  % 1 punkt wspolny
  dist = norm([x1 - x2, y1 - y2]);  if r1 + r2 < dist  r1 = r1 + (dist - r1 - r2)/2;  r2 = dist - r1 + 0.01*dist;  end
   [x, y] = circcirc(x1, y1, r1, x2, y2, r2);    % Sprawdzamy ile jest punktow przeciecia
  if isnan(x(1))  point = zeros(1,2);  elseif x(1) == x(2) && y(1) == y(2)  point = [x(1), y(1)];  else  % Wybieramy blizszy punktowi 3
  d1 = norm([x(1) - x3, y(1) - y3]);  d2 = norm([x(2) - x3, y(2) - y3]);  if d1 < d2  point = [x(1), y(1)];  else  point = [x(2), y(2)];  end
  end
    point = roundn(point, -3);  %point = real(point);
 end
  function notify(cur_node)
  %% Powiadamianie sasiadow o zmianie pozycji
  global g1400_LMap_next;
  global N_Map;
    emit_data(cur_node,false);  L = N_Map(cur_node,1,1);  for i = 2:L+1  nn = N_Map(cur_node, i, 1); % id sasiada
  if ~nn  continue;
  end
  g1400_LMap_next(nn,1) = 1; % do obrobienia w nastepnej iteracji
  recv_data(nn); % localization_draw_point(nn, g1400_LMap_next(nn, 2), 1);
  end
 end
   function [e] = get_e(p, W, T, F)
  %% Obliczanie bledu e
  e = sqrt((p(1)-W(1))^2 + (p(2)-W(2))^2) + sqrt((p(1)-T(1))^2 + (p(2)-T(2))^2) + sqrt((p(1)-F(1))^2 + (p(2) - F(2))^2); end
  function [type] = node_type(e)
  %% Obliczanie typu wezla na podstawie bledu e
  global g1400_prog1;
  global g1400_prog2;
    if e > g1400_prog2  type = 0; % do niczego
  end
  if e < g1400_prog1  type = 2; % latarnia
  end
  if e >= g1400_prog1 && e <= g1400_prog2  type = 1; % kotiwca
  end
  if e == 0  type = -1;  end
 end
  function draw_wtf(cur_node, W, T, F, anchors, r)
  global S_Map;
   plot3(S_Map(anchors(1),1), S_Map(anchors(1),2),300,'yellow.');  localization_draw_circle(r(1), S_Map(anchors(1),1), S_Map(anchors(1),2));  plot3(S_Map(anchors(2),1), S_Map(anchors(2),2),300,'yellow.');  localization_draw_circle(r(2), S_Map(anchors(2),1), S_Map(anchors(2),2));  plot3(S_Map(anchors(3),1), S_Map(anchors(3),2),300,'yellow.');  localization_draw_circle(r(3), S_Map(anchors(3),1), S_Map(anchors(3),2));    plot3(W(1), W(2),300,'green.');  X1=[W(1),S_Map(cur_node,1)];  Y1=[W(2),S_Map(cur_node,2)];  Z1=[300,S_Map(cur_node,3)];  line(X1,Y1,Z1,'Color',[0,1,1]);  plot3(T(1), T(2),300,'green.');  X1=[T(1),S_Map(cur_node,1)];  Y1=[T(2),S_Map(cur_node,2)];  Z1=[300,S_Map(cur_node,3)];  line(X1,Y1,Z1,'Color',[0,1,1]);  plot3(F(1), F(2),300,'green.');  X1=[F(1),S_Map(cur_node,1)];  Y1=[F(2),S_Map(cur_node,2)];  Z1=[300,S_Map(cur_node,3)];  line(X1,Y1,Z1,'Color',[0,1,1]); end
   function minangle = minAng(p1, p2, p3)
  minangle = min([ang(p1, p2, p3) ang(p1, p3, p2) ang(p3, p1, p2)]); end
  function angle = ang(p1, p2, p3)
  ab = dist(p1, p2);  bc = dist(p2, p3);  ac = dist(p1, p3);    cosang = (ab*ab+bc*bc-ac*ac)/(2*ab*bc);    angle = acos(cosang); end
  function distance = dist(p1, p2)
  distance = sqrt((p1(1) - p2(1))^2 + (p1(2) - p2(2))^2); end
 